#ifndef DATA_TYPE_DEFINE_HPP
#define DATA_TYPE_DEFINE_HPP
#include <chrono>
#include <memory>
#include <iostream>
#include <vector>
#include <math.h>
#define LUN_R_F 2
#define LUN_R_B 3
#define LUN_L_F 1
#define LUN_L_B 0
#define LINE_MAX_SPEED 8096.0  //直行最大速度(转速*10)
#define CIRCLE_MAX_SPEED 2500.0 //拐弯最大速度(转速*10)
inline double  calcDistance(double x, double y)
{
    return sqrt(x * x + y * y);
} 

inline int sgn(double x) 
{
    if(x >= 0) return 1;
    if(x < 0) return -1;

    return 0;
}
class Data_Class
{
 public:
  Data_Class();
  ~Data_Class(){}
  void data_msg_whell_control(std::vector <int32_t> &msg);
  void data_info_wheel_msg(std::vector <int32_t> &msg);
  void data_msg_to_indata(std::vector <int32_t> &msg);
  void data_control_to_msg(std::vector <int32_t> &msg);
 //wheel out data
    std::vector <int32_t> speedcur{8,0};
    std::vector <int32_t> anglecur{4,0}; 
    std::vector <int32_t> yaogan{4,0};
    std::vector <int32_t> otherout{8,0};
 //wheel in data
    std::vector <int32_t> speedset{8,0};
    std::vector <int32_t> motorother{8,0};
    std::vector <int32_t> angleset{4,0};
  
      //速度计算
       int vx,vy = 0, wz=0;                                //接收来自遥控的命令
        float cur_err,last_err,prev_err[20];
        double angle_c[4];                                    //计算结果-角度
        double angle_b[4];                                    //最优结果-角度

        double angle_n[4];                                    //当前结果-角度

        double rpm_c[4];                                      //计算结果-速度
        double rpm_b[4];       
        void yaokong(void);                                
        void Driver_cal(void); //最优结果-速度 
        
        int Driver_tiao(void);
        void Feedback_cal(void);
        double max_speed=4096.0f;
        double angle_shift[4];
   //车体参数     
      double WHEEL_RADIUS = 0.100; //轮半径
      double WHEEL_BASE =0.860 ;          // 车长（轮距） 0.860;
      double TRACK =0.690;               // 车宽（轮距）  0.690;

      double RY = WHEEL_BASE / 2.f;
      double RX = TRACK / 2.f;
      float wmga_max = 0.6;
      float line_x = 1.0;  //1m/s
    //将轮速 rmp 转换成 m/s
      float convert_rmp_to_ms(int sp); 
    //将轮速m/s 转换成  rmp  
       int convert_ms_to_rmp(float fsp) ;  
      void yaokong_real(float linex,float wmga,float l_y); //linex 米/秒 wmga 角速度  弧度/秒
      void Driver_cal_real(float linex,float wmga,float l_y);
      float convert_yaogan_wmga( int16_t w);
      float convert_yaogan_line(int16_t v);
      int get_module_speed_cur(int nob);
     // void cal_cur_speed_all(void);
      float cur_vx,cur_vy,cur_wz;
};


#endif
